#pragma once
#include <eigen3/Eigen/Dense>
#include <iostream>

class KalmanFilter {
 public:
  KalmanFilter();
  ~KalmanFilter();

  void Initialization(Eigen::VectorXd x_in);
  bool IsInitialized();

  // 5个矩阵
  void SetF(Eigen::MatrixXd F_in);
  void SetP(Eigen::MatrixXd P_in);
  void SetQ(Eigen::MatrixXd Q_in);
  void SetH(Eigen::MatrixXd H_in);
  void SetR(Eigen::MatrixXd R_in);

  //预测
  void Prediction();

  // KF更新
  void KFUpdate(Eigen::MatrixXd z);

  // EKF更新
  void EKFUpdate(Eigen::MatrixXd z);

  //获得状态x
  Eigen::MatrixXd GetX();

 private:
  // Jacobian矩阵
  void CalculateJacobianMatrix();

  bool is_initialized_;

  // 状态向量x
  Eigen::VectorXd x_;

  // 状态协方差矩阵P
  Eigen::MatrixXd P_;
  // 状态转移矩阵F
  Eigen::MatrixXd F_;
  // 过程噪声Q
  Eigen::MatrixXd Q_;
  // 测量矩阵H
  Eigen::MatrixXd H_;
  // 测量噪声R
  Eigen::MatrixXd R_;
};
